
/*
 * PID.c
 *
 *  Created on: Jun 10, 2024
 *      Author: Liangmiyi
 */

#include <MY_INC/PHY/PHY_PID.h>

#include <math.h> // 用于fabsf等数学函数

// 初始化PI控制器
static inline void AntiWindup_PID_Reset_Inline(AntiWindupPID__Typedef *pid);
void AntiWindup_PID_Init(AntiWindupPID__Typedef *pid, float Kp, float Ki, float Kd, const float ts, float min,
                         float max)
{
    if (pid == NULL)
        return; // 检查指针是否为 NULL
    pid->kp = Kp;
    pid->ki = Ki * 0.5f * ts; // 应用MATLAB中的0.5系数
    pid->kd = Kd;
    pid->min = min;
    pid->max = max;
    pid->kaw = 0.005f * pid->ki; // 应用MATLAB中的计算公式
    // 重置内部状态
    AntiWindup_PID_Reset_Inline(pid);
}

// 重置控制器状态
void AntiWindup_PID_Reset(AntiWindupPID__Typedef *pid)
{
    if (pid == NULL)
        return; // 检查指针是否为 NULL
    AntiWindup_PID_Reset_Inline(pid);
}
static inline void AntiWindup_PID_Reset_Inline(AntiWindupPID__Typedef *pid)
{
    pid->IntegralIterm = 0.0f;
    pid->lastError = 0.0f;
    pid->currentError = 0.0f;
    pid->out = 0.0f;
    pid->antiWindupError = 0.0f;
    pid->antiWindupLastError = 0.0f;
    pid->Differential = 0.0f;
}
/*==================================增量式PID算法===================================*/
/**
 * @brief 增量式PID初始化函数
 * @param PID 增量式PID结构体指针
 * @param Kp 比例增益
 * @param Ki 积分增益
 * @param Kd 微分增益
 * @param output_min PID输出最小值
 * @param output_max PID输出最大值
 */
void Increasing_PI_Init(IncrementalPID_Typedef *PI, float Kp, float Ki, const float ts, float output_min,
                        float output_max)
{
    if (PI == NULL)
        return;
    PI->ERROR = 0.0f;
    PI->ERROR_Last = 0.0f;
    PI->Out = 0.0f;
    PI->A0 = (Kp + Ki * ts * 0.5f);
    PI->B0 = (Ki * ts * 0.5f - Kp);
    PI->output_min = output_min;
    PI->output_max = output_max;
}
// 注意看增量式A0 B0 系数与差分方程系数的初始化，是一致的
void PI_Init(PI_Typedef *PI, float Kp, float Ki, float ts, float output_min, float output_max)
{
    PI->Coeff_B0 = (2.0f * Kp + Ki * ts) / 2.0f;  // 设置系数
    PI->Coeff_B1 = -(2.0f * Kp - Ki * ts) / 2.0f; // 设置系数
    PI->Yn_1 = 0.0f;
    PI->Xn_1 = 0.0f;
    PI->output_min = output_min; // 设置输出最小值
    PI->output_max = output_max; // 设置输出最大值
}
